#include "pid.h"

#include <string.h>
/*

* 函数介绍：旋转位置式PID计算
* 输入参数：p（PID调节器参数）err（误差值）
* 输出参数：
* 返回值  ：

*/
int positionPIDCalc_rotAngle(PIDTypeDef *p, int err) {
  int dError, error;
  int out;
  error = err;                           // 偏差
  p->sumError += error;                  // 积分
  dError = p->lastError - p->prevError;  // 当前微分
  p->prevError = p->lastError;           // 更新error[-1]的值
  p->lastError = error;                  // 更新error[-2]的值
  out = p->kp * error                    // 比例项
        + p->ki * p->sumError            // 积分项
        + p->kd * dError;                // 微分项

  out = limit(out, -p->limit, p->limit);

  return (out);
}

/*

* 函数介绍：位置式PID计算
* 输入参数：p（PID调节器参数）nextVaule（采样值）
* 输出参数：
* 返回值  ：

*/
int positionPIDCalc(PIDTypeDef *p, int nextValue) {
  int dError, error;
  int out;
  error = p->setValue - nextValue;       // 偏差
  p->sumError += error;                  // 积分
  dError = error - p->lastError;  // 当前微分
  p->prevError = p->lastError;           // 更新error[-1]的值
  p->lastError = error;                  // 更新error[-2]的值
  out = p->kp * error                    // 比例项
        + p->ki * p->sumError            // 积分项
        + p->kd * dError;                // 微分项

  out = limit(out, -p->limit, p->limit);

  return (out);
}
/**
 * @brief 积分分离PID计算
 * 
 * @param p pid调节器
 * @param nextValue 采样值
 * @param threshold 阈值
 * @return int 
 * @note 当误差绝对值大于阈值时，清零积分的累计误差
 */
int positionPiD_IntegralSeparate(PIDTypeDef *p, int nextValue,uint16_t threshold){
  int16_t dError, error;
  int out;
  error = p->setValue - nextValue;       // 偏差
  p->sumError += error;                  // 积分
  dError = error - p->lastError;  // 当前微分
  p->prevError = p->lastError;           // 更新error[-1]的值
  p->lastError = error;                  // 更新error[-2]的值
  //当误差大于阈值时，清零积分项
  if(abs16(error)>=threshold){
    p->sumError = 0;
  }
  out = p->kp * error                    // 比例项
        + p->ki * p->sumError            // 积分项
        + p->kd * dError;                // 微分项

  out = limit(out, -p->limit, p->limit);

  return (out);
}
/*

* 函数介绍：增量式PID计算
* 输入参数：p（PID调节器参数）nextVaule（采样值）
* 输出参数：
* 返回值  ：

*/
int incrementalPIDcalc(PIDTypeDef *p, int nextVaule) {
  int error;
  int ep, ei, ed;
  int out;
  error = p->setValue - nextVaule;  // 偏差
  ep = error - p->lastError;
  ei = error;
  ed = error - 2 * p->lastError + p->prevError;

  p->prevError = p->lastError;  // 更新error[-1]的值
  p->lastError = error;         // 更新error[-2]的值

  out = p->kp * ep     // 比例项
        + p->ki * ei   // 积分项
        + p->kd * ed;  // 微分项

  out = limit(out, -p->limit, p->limit);

  return (out);
}
/**
* 函数介绍：对PID调节器参数进行设置
* 输入参数：pidRegulator(待设置的PID调节器),setValue(期望值),limit(对输出的值进行限定),kp(比例系数),ki(积分系数),kd(微分系数)
* 输出参数：
* 返回值  ：
* 其他		：例如limit =
100则代表输出值的范围在+-100之间，意味着左右两轮的PWM相差不能超过1000
*/
void PID_Init(PIDTypeDef *pidRegulator, int setValue, int limit, float kp, float ki,
              float kd) {
  memset(pidRegulator, 0, sizeof(PIDTypeDef));  // 对PID结构体清零

  pidRegulator->setValue = setValue;
  pidRegulator->limit = limit;
  pidRegulator->kp = kp;
  pidRegulator->ki = ki;
  pidRegulator->kd = kd;
}

/*

* 函数介绍：位置式PID计算,并限制积分项
* 输入参数：p（PID调节器参数）nextVaule（采样值）
* 输出参数：
* 返回值  ：
* date : 2023/11/11

*/
int positionPIDCalc_LimitSumError(PIDTypeDef *p, int nextValue) {
  int dError, error;
  int out;
  error = p->setValue - nextValue;       // 偏差
  p->sumError += error;                  // 积分
  dError = p->lastError - p->prevError;  // 当前微分
  p->prevError = p->lastError;           // 更新error[-1]的值
  p->lastError = error;                  // 更新error[-2]的值
  p->sumError =
      limit(p->sumError, (float)-p->limit / p->ki,
            (float)p->limit / p->ki);  // 限制积分项的输出最大为limit，可以快速回调
  out = p->kp * error          // 比例项
        + p->ki * p->sumError  // 积分项
        + p->kd * dError;      // 微分项

  out = limit(out, -p->limit, p->limit);

  return (out);
}
